#include <ros/ros.h>
#include <tf/transform_listener.h>
//#include <turtlesim/Velocity.h>
//#include <turtlesim/Spawn.h>
#include <iostream>
#include "Localization_Env_Read.h"
#include "Localization.h"

using namespace std;
 
int main(int argc, char** argv)
{
   SquareInfo square;


   cout << "Hello World" << endl;
   ros::init(argc, argv, "my_tf_listener");

   Localization_Env_Read vars;
   Localization localization(vars.GetXGridSize(), vars.GetYGridSize(),
                             vars.GetXSquareSize(), vars.GetYSquareSize(),
                             vars.GetDeadZoneSize(), vars.GetPathWidthSize(),
                             vars.GetAngleOffset());

   localization.Update(0,0,0,0);

   square = localization.GetSquareInfo(0,0);


//   ros::NodeHandle node;

//   ros::service::waitForService("spawn");
//   ros::ServiceClient add_turtle = 
//        node.serviceClient<turtlesim::Spawn>("spawn");
//   turtlesim::Spawn srv;
//   add_turtle.call(srv);
// 
//   ros::Publisher turtle_vel = 
//        node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10);
// 
//   tf::TransformListener listener;
// 
//   ros::Rate rate(10.0);
//   while (node.ok())
//   {
//     tf::StampedTransform transform;
//     try
//     {
//       listener.lookupTransform("/turtle2", "/turtle1",  
//                                ros::Time(0), transform);
//     }
//     catch (tf::TransformException ex)
//     {
//       ROS_ERROR("%s",ex.what());
//     }
//
//     turtlesim::Velocity vel_msg;
//     vel_msg.angular = 4 * atan2(transform.getOrigin().y(),
//                                 transform.getOrigin().x());
//     vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
//                                  pow(transform.getOrigin().y(), 2));
//     turtle_vel.publish(vel_msg);
// 
//     rate.sleep();
//   }
//
   return 0;
};


